/*
 * @Author: your name
 * @Date: 2021-10-21 16:56:18
 * @LastEditTime: 2021-10-27 17:29:19
 * @LastEditors: Please set LastEditors
 * @Description: In User Settings Edit
 * @FilePath: \X\1km\driver\sensor\encode\enc_brt_can.c
 */
#include "enc_brt_can.h"
#include "../1km/bsp/canbus.h"
#include "string.h"

/* 本驱动程序用于读取布瑞特CAN类型编码器多圈值,并将其转换为机械距离信息
 * 通过累计两次读取位置的变化量,计算得出距离信息(这样的好处第一是允许编码器
 * 突然断电重启造成虚拟多圈值丢失情况而不影响距离累计,并且检测出虚拟多圈值
 * 丢失事件。第二是设置零点简单,不用通过命令设置编码器),同时当某个编码器
 * 由于重置线短路造成意外恢复出厂设置后,在预设的离线检测时间(1s)到达时会尝试
 * 自动修复,整个修复时间在300ms以内(由于此编码器重置需要短接2分钟,因此即便是
 * 反复被出厂设置也可正常工作)。
 * 
 * 速度过快事件
 * 意外恢复出厂事件
 * 掉线事件(掉线时长统计)
 * 通信失败率统计
 * 
*/


#define ABS(n)  (n > 0 ? (n) : (-n))

typedef struct{
    canbus_dev_t    dev;            //can设备

    uint32_t        readCnt;        //读取计数
    uint32_t        ackCnt;         //回应计数

    int32_t         curCount;       //多圈值(电子圈数x单圈精度)
    int32_t         lastCount;      //上次多圈值(用于异常判断)
    float           curPos;         //变换之后的当前位置
    float           transRatio;     //变换比例(通常是编码器值转换为mm系数)
    bool            isOffLine;      //是否离线
    bool            isFault;        //是否异常(主要是异常断电导致虚拟多圈值丢失造成位置突变)

    uint64_t        updateTime;     //更新时间戳
}Type_Enc_Def;


/**********************************************************/
aos_mutex_t mutex;

Type_Enc_Def EncTable[] = {
    //X-轴向编码器
    [0].dev.rx_id = 0x02,
    [0].transRatio = 0.097f,
    [0].isOffLine = true,

    //Y-轴向编码器
    [1].dev.rx_id = 0x03,
    [1].transRatio = 0.097f,
    [1].isOffLine = true,

    //Z-轴向编码器
    [2].dev.rx_id = 0x04,
    [2].transRatio = 0.097f,
    [2].isOffLine = true,


    //此dev用于检测是否有编码器被意外恢复出厂设置(恢复后ID变为1)
    [3].dev.rx_id = 0x01,
};

//掉线事件回调
void (*Event_OffLine_Callback)(uint8_t id, void *arg) = NULL;
//速度过快事件回调
void (*Event_SpeedTooFast_Callback)(uint8_t id, void *arg) = NULL;
//恢复出厂事件回调
void (*Event_Factory_Callback)(uint8_t id) = NULL;
/**********************************************************/


void xp_enc_brt_can_process_thread(void *arg);

uint8_t _cntBit1_uint32_(uint32_t num);
int8_t _bsr_uint32_(uint32_t num);
int32_t _get_pos_diff_(int32_t curCount, int32_t lastCount);
int32_t xp_enc_brt_can_send(canbus_dev_t *pdev, can_frame_t *frame);
can_frame_t *encode_multCount_frame(uint8_t id, can_frame_t *pFrame);
can_frame_t *encode_set_id_frame(uint8_t src_id, uint8_t des_id, can_frame_t *pFrame);
static int xp_enc_brt_can_debug(char *type,char *fun,char *param);



//初始化所有编码器,但凡有一个初始化失败将返回false,并且不会创建任务
bool xp_enc_brt_can_init(void)
{
    int rc = 0;
	can_frame_t frame;

    //can通道1总线初始化,别的地方有初始化这里就不用了
	rc = canbus_init();
	if(0 != rc){
		return false;
	}

    //注册所有编码器
    for (int i = 0; i < sizeof(EncTable) / sizeof(Type_Enc_Def); i++){
        EncTable[i].dev.rx_buf = (uint8_t *)aos_malloc(8);
        if (NULL != EncTable[i].dev.rx_buf){
            rc = canbus_dev_register(&EncTable[i].dev);
	        if (rc < 0) {
		        aos_free(EncTable[i].dev.rx_buf);
		        println("Enc node id <%d> register failed!", EncTable[i].dev.rx_id);
                return false;
	        }
        }else{
            println("Enc node id <%d> malloc failed!", EncTable[i].dev.rx_id);
            return false;
        }
    }

    //创建用于突发修改配置时所用的互斥锁
    if (0 != aos_mutex_new(&mutex)) {
        return false;
    }

    //创建一个用于轮询和解析编码器的任务
	if (0 != aos_task_new("xp_enc_brt_can_process_thread", xp_enc_brt_can_process_thread, NULL, 2048)) {
		println("create xp_enc_brt_can_process_thread fail");
		return false;
	}

    //添加调试接口
    xp_cli_debug_add(xp_enc_brt_can_debug);

	return true;
}

//编码器读取接收任务
void xp_enc_brt_can_process_thread(void *arg)
{
    can_frame_t frame = {0};

	while(1){

        aos_mutex_lock((aos_mutex_t *)&mutex, AOS_WAIT_FOREVER);

        for (int i = 0, hasOffline = 0; i < sizeof(EncTable) / sizeof(Type_Enc_Def); i++){

            if (sizeof(EncTable) / sizeof(Type_Enc_Def) - 1 == i){
                //如果未检测到有编码器读取失败,则跳过出厂恢复检测通道
                //修复仅能在只有一个编码器异常的情况下进行,同时存在多个则不能修复
                if (0 == hasOffline || _cntBit1_uint32_(hasOffline) != 1){
                    continue;
                }
            }

            //轮询电子多圈值
            if (0 != xp_enc_brt_can_send(&EncTable[i].dev, encode_multCount_frame(EncTable[i].dev.rx_id, &frame))){
                continue;
            }

            EncTable[i].readCnt++;

            //考虑到操作系统任务切换等的延时,这里将超时时间扩大到50ms
            if (aos_sem_wait((aos_sem_t *)&EncTable[i].dev.sem_t, 50) == 0){

                //(由于在未连接编码器的情况下,会收到自己发送的数据)这里通过CMD和DLC长度进行匹配
                if (CMD_GET_MULT_COUNT == EncTable[i].dev.rx_buf[2] && 7 == EncTable[i].dev.rx_len){

                    //以下范围内是对恢复出厂后的编码器进行修复的操作,如果不需要,删除即可
                    /********************************************************************************/
                    //如果是用于检测编码器异常的通道接收到数据,说明有编码器ID变为1了,进行下一步修复操作
                    if (sizeof(EncTable) / sizeof(Type_Enc_Def) - 1 == i){
                        if (NULL != Event_Factory_Callback) { Event_Factory_Callback(EncTable[_bsr_uint32_(hasOffline)].dev.rx_id); }
                        println("Found encode nodeId <%d> has been restored to the factory, Now, Try to fix\r\n", EncTable[_bsr_uint32_(hasOffline)].dev.rx_id);
                        if (0 == xp_enc_brt_can_send(&EncTable[sizeof(EncTable) / sizeof(Type_Enc_Def) - 1].dev, encode_set_id_frame(0x01, EncTable[_bsr_uint32_(hasOffline)].dev.rx_id, &frame))){
                            println("Node id <%d> modify to <%d> cmd send ok\r\n", 0x01, EncTable[_bsr_uint32_(hasOffline)].dev.rx_id);
                        }else{
                            println("Node id <%d> modify to <%d> cmd send failed\r\n", 0x01, EncTable[_bsr_uint32_(hasOffline)].dev.rx_id);
                        }
                    }
                    /********************************************************************************/
                    else{
                        EncTable[i].ackCnt++;   //统计成功应答次数
                        EncTable[i].curCount = EncTable[i].dev.rx_buf[3] | (EncTable[i].dev.rx_buf[4] << 8) | (EncTable[i].dev.rx_buf[5] << 16) | (EncTable[i].dev.rx_buf[6] << 24);
                        
                        int32_t diff = _get_pos_diff_(EncTable[i].curCount, EncTable[i].lastCount);
                        EncTable[i].lastCount = EncTable[i].curCount;
                        //检测意外断电导致的虚拟多圈丢失
                        if (ABS(diff) > MAX_ALLOW_RANGE(i)){
                            EncTable[i].isFault = true;

                            float speed = diff * EncTable[i].transRatio * 1000.0f / (aos_now_ms() - EncTable[i].updateTime);  //速度单位mm/s
                            if (NULL != Event_SpeedTooFast_Callback) {  Event_SpeedTooFast_Callback(EncTable[i].dev.rx_id, &speed); }

                            println("Encode node id <%d> value fault, diff:%d, speed:%.2f mm/s\r\n", EncTable[i].dev.rx_id, diff, speed);
                            continue;   //异常出现直接跳过更新当前位置即可,后面数据正常后会正常累计
                        }else{
                            EncTable[i].isFault = false;
                        }
                        
                        //用累计增量的方式可对意外断电造成虚拟多圈丢失情况允许，并且将设置零点变得更简单方便
                        EncTable[i].curPos += diff * EncTable[i].transRatio;

                        EncTable[i].updateTime = aos_now_ms();
                        //print("Node id <%d> curPos = %.0f\r\n", EncTable[i].dev.rx_id, EncTable[i].curPos);
                    }
                }else{
					/*
                    println("Recv %02X %02X %02X %02X %02X %02X %02X %02X\r\n", EncTable[i].dev.rx_buf[0], EncTable[i].dev.rx_buf[1], 
                                                                            EncTable[i].dev.rx_buf[2], EncTable[i].dev.rx_buf[3], 
                                                                            EncTable[i].dev.rx_buf[4], EncTable[i].dev.rx_buf[5],
                                                                            EncTable[i].dev.rx_buf[6], EncTable[i].dev.rx_buf[7]);
					*/
                }
		    }else{
				//RT1052 CAN配置或者底层代码问题 不接设备会收到发送的报文,所以这里的No response检测用不了
                println("Dev node id <%d> not response", EncTable[i].dev.rx_id);		
            }

            //不对测试通道进行离线判断
            if (sizeof(EncTable) / sizeof(Type_Enc_Def) - 1 != i){

                uint32_t offLineTime = aos_now_ms() - EncTable[i].updateTime;
                if (offLineTime > OFFLINE_TIME_MS){
                    
                    if (!EncTable[i].isOffLine){
                        EncTable[i].isOffLine = true;
                        if (NULL != Event_OffLine_Callback) { Event_OffLine_Callback(EncTable[i].dev.rx_id, &offLineTime); }
                    }
                    hasOffline |= 0x01 << i;    //置位离线bit标志
                    println("Enc Node id <%d> isOffLine, It's been %ld s\r\n", EncTable[i].dev.rx_id, offLineTime);
                }else{
                    EncTable[i].isOffLine = false;
                }
            }
        }

        aos_mutex_unlock((aos_mutex_t *)&mutex);

        aos_msleep(20);
	}
}

//内部功能函数
/******************************************************************************/

//can发送接口
int32_t xp_enc_brt_can_send(canbus_dev_t *pdev, can_frame_t *frame)
{
	if (NULL == frame ||
		NULL == pdev->bus ||
		NULL == pdev->bus->canbus_send) {
		return -1;
	}
	return pdev->bus->canbus_send(frame);
}
//封装读取多圈命令数据包
can_frame_t *encode_multCount_frame(uint8_t id, can_frame_t *pFrame)
{
    if (NULL != pFrame){
        //以下是非标准SDO包,用于读取编码器值(电子圈数x单圈精度)
        pFrame->header.id = id;                 //节点ID
        pFrame->header.dlc = 0x04;              //DLC长度
        pFrame->data[0] = 0x04;                 //数据长度(非标协议)
        pFrame->data[1] = id;                   //节点ID(非标协议)
        pFrame->data[2] = CMD_GET_MULT_COUNT;   //指令码(非标协议)
        pFrame->data[3] = 0x00;                 //数据0
    }
    return pFrame;
}
//封装设置Id命令数据包
can_frame_t *encode_set_id_frame(uint8_t src_id, uint8_t des_id, can_frame_t *pFrame)
{
    if (NULL != pFrame){
        //以下是非标准SDO包,用于修改节点ID
        pFrame->header.id = src_id;             //节点ID
        pFrame->header.dlc = 0x04;              //DLC长度
        pFrame->data[0] = 0x04;                 //数据长度(非标协议)
        pFrame->data[1] = src_id;               //节点ID(非标协议)
        pFrame->data[2] = CMD_SET_ENC_ID;   	//指令码(非标协议)
        pFrame->data[3] = des_id;               //目标Id
    }
    return pFrame;
}
//从低位往左找出第一个出现1的位置(皆为0返回-1)
inline int8_t _bsr_uint32_(uint32_t num){
    int8_t count = (sizeof(num) << 3) - 1;
    for(uint32_t mask = 1U << count; !(num & mask) && count >= 0; count--, mask >>= 1);
    return count;
}
//找出1的个数
inline uint8_t _cntBit1_uint32_(uint32_t num){
    uint8_t count = 0;
    for(; num != 0; num = num & (num - 1), count++);
    return count;
}
//获取编码器增量值(主要是处理溢出)
int32_t _get_pos_diff_(int32_t curCount, int32_t lastCount)
{
    int32_t diff;
    diff = curCount - lastCount;

	if (diff > ENC_BRT_CAN_MAX_CNT / 2) {
		diff -= ENC_BRT_CAN_MAX_CNT;
	}
	else if (diff < -ENC_BRT_CAN_MAX_CNT / 2) {
		diff += ENC_BRT_CAN_MAX_CNT;
	}
    return diff;
}



//外部接口
/*********************************************************************************/

//修改编码器ID. from src_id to des_id
bool xp_enc_brt_can_set_nodeID(uint8_t src_id, uint8_t des_id)
{
    can_frame_t frame = {0};

    if (0 == src_id || src_id > 127 || 0 == des_id || des_id > 127){
        return false;
    }
	
	bool ret = false;
	
    aos_mutex_lock((aos_mutex_t *)&mutex, AOS_WAIT_FOREVER);
    //通过异常检测通道发送消息
    if (0 == xp_enc_brt_can_send(&EncTable[sizeof(EncTable) / sizeof(Type_Enc_Def) - 1].dev, encode_set_id_frame(src_id, des_id, &frame))){
        println("Node id <%d> modify to <%d> cmd send ok\r\n", src_id, des_id);
        ret = true;
    }else{
        println("Node id <%d> modify to <%d> cmd send failed\r\n", src_id, des_id);
    }
    
    //这里不对设置成功或失败的回应消息进行检测,因为设置成功后回应的消息其ID会变成des_id,上层canbus会根据此id生产相应通道的信号量，
    //要进行回应消息检测,就得知道nodeId和EncTable索引的映射关系,从而消费相应通道的信号量。没必要

    aos_mutex_unlock((aos_mutex_t *)&mutex);
	return ret;
}

//根据编号查询编码器是否离线(默认宏设置1s的离线检测时间)
bool xp_enc_brt_can_isOffline(uint8_t index)
{
    if (index < sizeof(EncTable) / sizeof(Type_Enc_Def) - 1){
        return EncTable[index].isOffLine;
    }
    return false;
}
//根据编号查询编码器读取失败率
bool xp_enc_brt_can_get_failRate(uint8_t index, float *pResult)
{
    if (index < sizeof(EncTable) / sizeof(Type_Enc_Def) - 1){
        if (0 != EncTable[index].readCnt){
            *pResult = 100 - (float)EncTable[index].ackCnt / EncTable[index].readCnt * 100;
        }else{
            *pResult = 100;
        }
        return true;
    }
    return false;
}
//根据编号清空编码器读取回应统计
bool xp_enc_brt_can_clear_failRate(uint8_t index)
{
    if (index < sizeof(EncTable) / sizeof(Type_Enc_Def) - 1){
        EncTable[index].ackCnt = 0;
        EncTable[index].readCnt = 0;;
        return true;
    }
    return false;
}
//根据编号查询编码器位置(经过变换比例之后的值)
bool xp_enc_brt_can_get_pos(uint8_t index, int32_t *pResult)
{
    if (index < sizeof(EncTable) / sizeof(Type_Enc_Def) - 1 && NULL != pResult){
        *pResult = (int32_t)EncTable[index].curPos;
        return true;
    }
    return false;
}
//根据编号设置编码器当前位置为零点
bool xp_enc_brt_can_set_zero(uint8_t index)
{
    if (index < sizeof(EncTable) / sizeof(Type_Enc_Def) - 1){
        EncTable[index].curPos = 0;
        return true;
    }
    return false;
}



//调试相关
/********************************************************************************/
void xp_enc_can__show(void)
{
    for (int i = 0; i < sizeof(EncTable) / sizeof(Type_Enc_Def); i++){
        println("Node id <%d> isOffline: %s  curPos: %.0f  readCnt: %d  ackCnt: %d", EncTable[i].dev.rx_id, EncTable[i].isOffLine ? ("OffLine") : ("OnLine"), EncTable[i].curPos, EncTable[i].readCnt, EncTable[i].ackCnt);
    }
}

static int xp_enc_brt_can_debug(char *type,char *fun,char *param)
{
	int ret;
	
	//本调试接口仅支持 "enc_can_" 起始的命令(不知道型号,没法命名了)
	if (NULL == strstr(type, "enc_can_")){
		return 0;
	}else{
		type += strlen("enc_can_");
	}
    //xp enc_can__show  打印所有编码器值
	if (strcasecmp(type, "Show") == 0){
		xp_enc_can__show();
	}
	else if (strcasecmp(type, "clear") == 0){
        println("Encode index <%d> clear failRate %s\r\n", atoi(fun), xp_enc_brt_can_clear_failRate(atoi(fun)) ? ("Ok") : ("Fail"));
    }
    else if (strcasecmp(type, "getPos") == 0){
        int32_t result;
        xp_enc_brt_can_get_pos(atoi(fun), &result);
        println("Encode index <%d> pos is: %d\r\n", atoi(fun), result);
    }
    else if (strcasecmp(type, "setZero") == 0){
        println("Encode index <%d> setZero %s\r\n", atoi(fun), xp_enc_brt_can_set_zero(atoi(fun)) ? ("Ok") : ("Fail"));
    }
    else if (strcasecmp(type, "setId") == 0){
        xp_enc_brt_can_set_nodeID(atoi(fun), atoi(param));
    }
    else if (strcasecmp(type, "failRate") == 0){
        float result = -100;     //命令index错误返回值
        xp_enc_brt_can_get_failRate(atoi(fun), &result);
        println("Encode index <%d> failure rate is: %.2f\r\n", atoi(fun), result);
    }
    else {
        return 0;
    }
    return 1;
}